//#define _T_DEBUG 


#region Using Statements
using System;
using System.Collections.Generic;
using System.Text;
using Microsoft.Xna.Framework;
#endregion


namespace SkinnedModel
{
    /// <summary>
    /// IK Goal interface
    /// Yup - goals have a solve routine
    /// </summary>
    
    public interface IKGoal
    {
        void Solve(ref Skeleton s);
        //void GetControlledJoints(ref List<int> controlledJointIds, ref Skeleton s);
    }


    /// <summary>
    //A faux goal that tells the ik solver to apply 
    //an absolute rotation to a joint (ie move joint to this rotation)
    /// </summary>
    public class AbsoluteOrientationGoal : IKGoal
    {
        protected Quaternion q;
        protected int jointIdx;

        public AbsoluteOrientationGoal(int jointId, Quaternion orientation)
        {
            jointIdx = jointId;
            q = orientation;
        }

        public void Solve(ref Skeleton s)
        {
            s.joints[jointIdx].q = q;
        }

        /*
        public void GetControlledJoints(ref List<int> controlledJointIds, ref Skeleton s)
        {
            if (!controlledJointIds.Contains(jointIdx))
            {
                controlledJointIds.Add(jointIdx);
            }
        }
         */
    }

    /// <summary>
    //A faux goal that tells the ik solver to apply 
    //a relative rotation to a joint (rotate by q)
    /// </summary>
    public class RelativeOrientationGoal : IKGoal
    {
        protected Quaternion q;
        protected int jointIdx;

        public RelativeOrientationGoal(int jointId, Quaternion orientation)
        {
            jointIdx = jointId;
            q = orientation;
        }

        public void Solve(ref Skeleton s)
        {
            s.joints[jointIdx].q = Quaternion.Concatenate(s.joints[jointIdx].q, q);
        }

        /*
         * public void GetControlledJoints(ref List<int> controlledJointIds, ref Skeleton s)
        {
            if (!controlledJointIds.Contains(jointIdx))
            {
                controlledJointIds.Add(jointIdx);
            }
        }*/

    }

    /// <summary>
    /// Single joint IK goal
    /// currently computes an aim goal
    /// Useful for animating a head
    /// </summary>
    public class Goal1JointAim : IKGoal
    {
        protected Vector3 worldTargetAim;
        protected int effectorIdx;

        /// <summary>
        /// Given a jointID, a WORLD SPACE aim, 
        /// define an IK Goal
        /// </summary>
        /// <param name="jointID"></param>
        /// <param name="worldTargetAim"></param>        
        public Goal1JointAim(int jointID, Vector3 worldTargetAim)
        {
            effectorIdx = jointID;
            
            ///we input is a world space aim
            this.worldTargetAim = worldTargetAim;                        
            this.worldTargetAim.Normalize();
        }


        //Note this is an AIM goal, not a positional goal
        public void Solve(ref Skeleton s)
        {

            Vector3 effectorAim = new Vector3();
            s.GetEffectorAim(effectorIdx, out effectorAim);
#if _T_DEBUG
            Console.WriteLine("Solver current world aim:");
            Console.WriteLine(effectorAim);

            Console.WriteLine("Solver current world target aim:");
            Console.WriteLine(worldTargetAim);
#endif            
            int parentId = s.parents[effectorIdx];
            int ancestorId = s.parents[parentId];
           
            //get transform from root to local joint space            
            Quaternion world2local = Quaternion.Identity;

            //if (ancestorId > -1)
            //{
            s.GetJointRotation(parentId, out world2local);
            //}

#if _T_DEBUG

            //DEBUG
            Vector3 v = new Vector3();
            TMath.Quaternion2Eulers(ref world2local, out v);
            v *= 180.0f / MathHelper.Pi;
            Console.WriteLine("world2local"); Console.WriteLine(v);

            float dpD = Vector3.Dot(effectorAim, worldTargetAim);
            float ang = (float) Math.Acos((float)dpD);
            Console.WriteLine("Debug Angle"); Console.WriteLine(MathHelper.ToDegrees(ang));
            //DEBUG
#endif

            //transform vectors into local space
            Vector3 localEffectorAim = Vector3.Transform(effectorAim, world2local);
            Vector3 localTargetAim = Vector3.Transform(worldTargetAim, world2local);

            ///get rotation axis (TODO: consider constraints)
            Vector3 rAxis = Vector3.Cross(localEffectorAim, localTargetAim);//note that there seems to be some eff up in the orination (we get a negative angle)
            rAxis.Normalize();


            s.axis = Vector3.Transform(rAxis, Quaternion.Inverse(world2local));

#if _T_DEBUG
            //DEBUG
            Console.WriteLine("Solver axis:");
            Console.WriteLine(s.axis);
#endif
            ///Get rotation angle
            float dp = Vector3.Dot(localEffectorAim, localTargetAim);
            if (dp > 0.995)
            {
#if _T_DEBUG
                Console.WriteLine("Very Close to solve, exiting");
#endif
                return;
            }

            float angle = (float)Math.Acos((double)dp);
#if _T_DEBUG
            Console.WriteLine("Solver angle:");            
            Console.WriteLine(MathHelper.ToDegrees(angle));
#endif
            Quaternion qr = Quaternion.CreateFromAxisAngle(rAxis, angle);

            /*Matrix M = Matrix.CreateFromQuaternion(qr);
            Matrix MD = M;
            M.M31 = MD.M11;
            M.M32 = MD.M12;
            M.M33 = MD.M13;
            M.M11 = MD.M31;
            M.M12 = MD.M32;
            M.M13 = MD.M33;

            Vector3 sc = new Vector3();
            Vector3 t = new Vector3();
            M.Decompose(out sc, out qr, out t);
            */

            localEffectorAim = Vector3.Transform(localEffectorAim, qr);

#if _T_DEBUG
            //DEBUG
            v = new Vector3();
            TMath.Quaternion2Eulers(ref qr, out v);
            v *= 180.0f / MathHelper.Pi;
            Console.WriteLine("qr rotations: "); Console.WriteLine(v);
            //DEBUG
#endif

           //qr = Quaternion.CreateFromYawPitchRoll(0, 0, MathHelper.ToRadians(90));

            s.joints[parentId].q = Quaternion.Concatenate(qr, s.joints[parentId].q);
            //s.joints[parentId].q = qr * s.joints[parentId].q;

            //DEBUG
#if _T_DEBUG            
            Vector3 ef = effectorAim;

            s.GetEffectorAim(effectorIdx, out ef);

            
            TMath.Quaternion2Eulers(ref s.joints[parentId].q, out v);
            v *= 180.0f / MathHelper.Pi;
            Console.WriteLine("final rotations: "); Console.WriteLine(v);
#endif
        }

        /*
        public void GetControlledJoints(ref List<int> controlledJointIds, ref Skeleton s)
        {
            int rootIdx = s.parents[effectorIdx];
            if (!controlledJointIds.Contains(rootIdx))
            {
                controlledJointIds.Add(rootIdx);
            }
        }
         */
    }


    public class ReachGoal : IKGoal{

        protected Vector3 worldTarget;
        protected int effectorIdx;
        protected int rootIdx;

        /// <summary>
        /// Given a jointID, a WORLD SPACE target, 
        /// define an IK Goal
        /// </summary>
        /// <param name="effectorId"></param>
        /// <param name="rootId"></param>
        /// <param name="worldTarget"></param>        
        public ReachGoal(int effectorId, int rootId, Vector3 worldTarget)
        {
            effectorIdx = effectorId;
            rootIdx = rootId;

            ///we input is a world space aim
            this.worldTarget= worldTarget;                        
        }

        public void Solve(ref Skeleton s)
        {

            List<Joint> chain = new List<Joint>();
            int jointId = effectorIdx;

            while (jointId != rootIdx)
            {
                jointId = s.parents[jointId];
                chain.Add(s.joints[jointId]);
            }

            float delta = 10.0f;
            bool hasProgress = true;

            Vector3 curPos = new Vector3();
            Vector3 stepPos= new Vector3();
            for (int i = 0; i < 20; i++)
            {
                //if we didn't make any progress, cut search size down
                if (!hasProgress)
                {
                    delta /= 2.0f;
                }

                //narrowed search down
                if (delta <= 0.1f)
                {
                    break;
                }

                hasProgress = false;

                Quaternion qx = Quaternion.CreateFromYawPitchRoll(delta, 0, 0);
                Quaternion qy = Quaternion.CreateFromYawPitchRoll(0, delta, 0);
                Quaternion qz = Quaternion.CreateFromYawPitchRoll(0, 0, delta);
                List<Quaternion> qList = new List<Quaternion>();
                qList.Add(qx);
                qList.Add(qy);
                qList.Add(qz);

                qList.Add(-qx);
                qList.Add(-qy);
                qList.Add(-qz);

                foreach (Joint j in chain)
                {

                    s.GetEffectorPosition(effectorIdx,out curPos);
                    
                    foreach (Quaternion q in qList)
                    {
                        //improve joint
                        Quaternion jOrig = j.q;
                        j.q *= q;
                        s.GetEffectorPosition(effectorIdx, out stepPos);
                        if (Vector3.Distance(worldTarget, curPos) <=
                            Vector3.Distance(worldTarget, stepPos))
                        {
                            //roll back change
                            j.q = jOrig;
                        }
                        else
                        {
                            //keep change
                            hasProgress = true;
                        }

                    }
                }

            }
        }

        /*
        /// <summary>
        /// Gets the chain under control
        /// </summary>
        /// <param name="controlledJointIds"></param>
        /// <param name="s"></param>
        public void GetControlledJoints(ref List<int> controlledJointIds, ref Skeleton s)
        {
            int currentIdx = effectorIdx;

            while (currentIdx != rootIdx)
            {
                currentIdx = s.parents[currentIdx];
                if (!controlledJointIds.Contains(currentIdx))
                {
                    controlledJointIds.Add(currentIdx);
                }
            }
        }
         * */
    }
}